<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN" "http://www.w3.org/TR/html4/loose.dtd">
<html lang="en">
<head>
<title>Source code</title>
<link rel="stylesheet" type="text/css" href="../../../../stylesheet.css" title="Style">
</head>
<body>
<div class="sourceContainer">
<pre><span class="sourceLineNo">001</span>package imagingbook.calibration.zhang;<a name="line.1"></a>
<span class="sourceLineNo">002</span><a name="line.2"></a>
<span class="sourceLineNo">003</span>import java.awt.geom.Point2D;<a name="line.3"></a>
<span class="sourceLineNo">004</span>import java.util.Arrays;<a name="line.4"></a>
<span class="sourceLineNo">005</span><a name="line.5"></a>
<span class="sourceLineNo">006</span>import org.apache.commons.math3.analysis.MultivariateMatrixFunction;<a name="line.6"></a>
<span class="sourceLineNo">007</span>import org.apache.commons.math3.analysis.MultivariateVectorFunction;<a name="line.7"></a>
<span class="sourceLineNo">008</span><a name="line.8"></a>
<span class="sourceLineNo">009</span>/**<a name="line.9"></a>
<span class="sourceLineNo">010</span> * Nonlinear optimizer based on the Levenberg-Marquart method, where the Jacobian matrix<a name="line.10"></a>
<span class="sourceLineNo">011</span> * is calculated numerically (i.e., by estimating the first partial derivatives from<a name="line.11"></a>
<span class="sourceLineNo">012</span> * finite differences). The advantage is that the calculation of the Jacobian is <a name="line.12"></a>
<span class="sourceLineNo">013</span> * independent of the calibration model, while performance and runtime are similar to<a name="line.13"></a>
<span class="sourceLineNo">014</span> * the analytic version (see {@link NonlinearOptimizerAnalytic}).<a name="line.14"></a>
<span class="sourceLineNo">015</span> * <a name="line.15"></a>
<span class="sourceLineNo">016</span> * @author WB<a name="line.16"></a>
<span class="sourceLineNo">017</span> */<a name="line.17"></a>
<span class="sourceLineNo">018</span>public class NonlinearOptimizerNumeric extends NonlinearOptimizer {<a name="line.18"></a>
<span class="sourceLineNo">019</span>        <a name="line.19"></a>
<span class="sourceLineNo">020</span>        protected NonlinearOptimizerNumeric(Point2D[] modelPts, Point2D[][] obsPts) {<a name="line.20"></a>
<span class="sourceLineNo">021</span>                super(modelPts, obsPts);<a name="line.21"></a>
<span class="sourceLineNo">022</span>        }<a name="line.22"></a>
<span class="sourceLineNo">023</span>        <a name="line.23"></a>
<span class="sourceLineNo">024</span>        @Override<a name="line.24"></a>
<span class="sourceLineNo">025</span>        MultivariateVectorFunction makeValueFun() {<a name="line.25"></a>
<span class="sourceLineNo">026</span>                return new ValueFun();<a name="line.26"></a>
<span class="sourceLineNo">027</span>        }<a name="line.27"></a>
<span class="sourceLineNo">028</span><a name="line.28"></a>
<span class="sourceLineNo">029</span>        @Override<a name="line.29"></a>
<span class="sourceLineNo">030</span>        MultivariateMatrixFunction makeJacobianFun() {<a name="line.30"></a>
<span class="sourceLineNo">031</span>                return new JacobianFun();<a name="line.31"></a>
<span class="sourceLineNo">032</span>        }<a name="line.32"></a>
<span class="sourceLineNo">033</span><a name="line.33"></a>
<span class="sourceLineNo">034</span>        private class JacobianFun implements MultivariateMatrixFunction {<a name="line.34"></a>
<span class="sourceLineNo">035</span>                <a name="line.35"></a>
<span class="sourceLineNo">036</span>                /**<a name="line.36"></a>
<span class="sourceLineNo">037</span>                 * Calculates a "stacked" Jacobian matrix with 2MN rows and K = 7 + 6M<a name="line.37"></a>
<span class="sourceLineNo">038</span>                 * columns (for M views with N points each, K parameters). For example, <a name="line.38"></a>
<span class="sourceLineNo">039</span>                 * with M = 5 views and N = 256 points each, J is of size 2560 × 37.<a name="line.39"></a>
<span class="sourceLineNo">040</span>                 * Each pair of rows in the Jacobian corresponds to one point.<a name="line.40"></a>
<span class="sourceLineNo">041</span>                 * THIS VERSION only calculates single blocks of the Jacobian!<a name="line.41"></a>
<span class="sourceLineNo">042</span>                 */<a name="line.42"></a>
<span class="sourceLineNo">043</span>                @Override<a name="line.43"></a>
<span class="sourceLineNo">044</span>            public double[][] value(double[] params) {<a name="line.44"></a>
<span class="sourceLineNo">045</span>                        final int K = params.length;<a name="line.45"></a>
<span class="sourceLineNo">046</span>                double[][] J = new double[2 * M * N][K];        // the Jacobian matrix (initialized to zeroes!)<a name="line.46"></a>
<span class="sourceLineNo">047</span>                double[] refValues = new double[2 * M * N];     // values obtained with undisturbed parameters <a name="line.47"></a>
<span class="sourceLineNo">048</span>                <a name="line.48"></a>
<span class="sourceLineNo">049</span>                double[] a = Arrays.copyOfRange(params, 0, camParLength);       // camera parameters<a name="line.49"></a>
<span class="sourceLineNo">050</span>                Camera camOrig = new Camera(a);<a name="line.50"></a>
<span class="sourceLineNo">051</span>                <a name="line.51"></a>
<span class="sourceLineNo">052</span>                // Step 0: calculate all 2MN reference output values (for undisturbed parameters)<a name="line.52"></a>
<span class="sourceLineNo">053</span>               <a name="line.53"></a>
<span class="sourceLineNo">054</span>                for (int r = 0, i = 0; i &lt; M; i++) {    // for all views, r = row<a name="line.54"></a>
<span class="sourceLineNo">055</span>                        int m = camParLength + viewParLength * i;<a name="line.55"></a>
<span class="sourceLineNo">056</span>                                double[] w = Arrays.copyOfRange(params, m, m + viewParLength);<a name="line.56"></a>
<span class="sourceLineNo">057</span>                                ViewTransform view = new ViewTransform(w);<a name="line.57"></a>
<span class="sourceLineNo">058</span>                        for (int j = 0; j &lt; N; j++) {   // for all model points: calculate reference values<a name="line.58"></a>
<span class="sourceLineNo">059</span>                                double[] uv = camOrig.project(view, modelPts[j]);<a name="line.59"></a>
<span class="sourceLineNo">060</span>                                refValues[r + 0] = uv[0];<a name="line.60"></a>
<span class="sourceLineNo">061</span>                                refValues[r + 1] = uv[1];<a name="line.61"></a>
<span class="sourceLineNo">062</span>                                r = r + 2;<a name="line.62"></a>
<span class="sourceLineNo">063</span>                        }                <a name="line.63"></a>
<span class="sourceLineNo">064</span>                }<a name="line.64"></a>
<span class="sourceLineNo">065</span>                <a name="line.65"></a>
<span class="sourceLineNo">066</span>                // Step 1: calculate the leftmost (green) block of J associated with camera intrinsics<a name="line.66"></a>
<span class="sourceLineNo">067</span>                <a name="line.67"></a>
<span class="sourceLineNo">068</span>                for (int k = 0; k &lt; a.length; k++) {    // for all camera parameters<a name="line.68"></a>
<span class="sourceLineNo">069</span>                        double ak = a[k];                                       // keep original parameter value        <a name="line.69"></a>
<span class="sourceLineNo">070</span>                        double delta = estimateDelta(ak);<a name="line.70"></a>
<span class="sourceLineNo">071</span>                        a[k] = a[k] + delta;            // modify parameter s_k<a name="line.71"></a>
<span class="sourceLineNo">072</span>                        Camera camMod = new Camera(a);  // modified camera<a name="line.72"></a>
<span class="sourceLineNo">073</span>                        <a name="line.73"></a>
<span class="sourceLineNo">074</span>                        for (int r = 0, i = 0; i &lt; M; i++) {    // for all views, r = row<a name="line.74"></a>
<span class="sourceLineNo">075</span>                                int m = camParLength + i * viewParLength;<a name="line.75"></a>
<span class="sourceLineNo">076</span>                                double[] w = Arrays.copyOfRange(params, m, m + viewParLength);<a name="line.76"></a>
<span class="sourceLineNo">077</span>                                ViewTransform view = new ViewTransform(w);<a name="line.77"></a>
<span class="sourceLineNo">078</span>                                for (int j = 0; j &lt; N; j++) {   // for all model points: calculate disturbed value<a name="line.78"></a>
<span class="sourceLineNo">079</span>                                        Point2D Pj = modelPts[j];<a name="line.79"></a>
<span class="sourceLineNo">080</span>                                        double[] uvMod = camMod.project(view, Pj);<a name="line.80"></a>
<span class="sourceLineNo">081</span>                                        J[r + 0][k] = (uvMod[0] - refValues[r + 0]) / delta;   // dX<a name="line.81"></a>
<span class="sourceLineNo">082</span>                                        J[r + 1][k] = (uvMod[1] - refValues[r + 1]) / delta;   // dY<a name="line.82"></a>
<span class="sourceLineNo">083</span>                                        r = r + 2;<a name="line.83"></a>
<span class="sourceLineNo">084</span>                                }    <a name="line.84"></a>
<span class="sourceLineNo">085</span>                        }<a name="line.85"></a>
<span class="sourceLineNo">086</span>                        a[k] = ak;      // return parameter s_k to original<a name="line.86"></a>
<span class="sourceLineNo">087</span>                }<a name="line.87"></a>
<span class="sourceLineNo">088</span>                <a name="line.88"></a>
<span class="sourceLineNo">089</span>                // Step 2: calculate the diagonal blocks, one for each view<a name="line.89"></a>
<span class="sourceLineNo">090</span>                <a name="line.90"></a>
<span class="sourceLineNo">091</span>                for (int i = 0; i &lt; M; i++) {   // for all views/blocks<a name="line.91"></a>
<span class="sourceLineNo">092</span>                        final int start = camParLength + i * viewParLength;<a name="line.92"></a>
<span class="sourceLineNo">093</span>                        double[] w = Arrays.copyOfRange(params, start, start + viewParLength);<a name="line.93"></a>
<span class="sourceLineNo">094</span>                        final int c = a.length + i * w.length;          // leftmost matrix column of block i<a name="line.94"></a>
<span class="sourceLineNo">095</span>                        for (int k = 0; k &lt; w.length; k++) {    // for all parameters in w<a name="line.95"></a>
<span class="sourceLineNo">096</span>                                double wk = w[k];                                       // keep original parameter w_k<a name="line.96"></a>
<span class="sourceLineNo">097</span>                                double delta = estimateDelta(wk);<a name="line.97"></a>
<span class="sourceLineNo">098</span>                                w[k] =  w[k] + delta;                           // modify parameter w_k<a name="line.98"></a>
<span class="sourceLineNo">099</span>                                ViewTransform view = new ViewTransform(w);<a name="line.99"></a>
<span class="sourceLineNo">100</span>                                int r = 2 * i * N;      // row<a name="line.100"></a>
<span class="sourceLineNo">101</span>                                for (int j = 0; j &lt; N; j++) {           // for all model points: calculate disturbed value<a name="line.101"></a>
<span class="sourceLineNo">102</span>                                        Point2D Pj = modelPts[j];<a name="line.102"></a>
<span class="sourceLineNo">103</span>                                        double[] uvMod = camOrig.project(view, Pj);<a name="line.103"></a>
<span class="sourceLineNo">104</span>                                        J[r + 0][c + k] = (uvMod[0] - refValues[r + 0]) / delta;   // dX<a name="line.104"></a>
<span class="sourceLineNo">105</span>                                        J[r + 1][c + k] = (uvMod[1] - refValues[r + 1]) / delta;   // dY<a name="line.105"></a>
<span class="sourceLineNo">106</span>                                        r = r + 2;<a name="line.106"></a>
<span class="sourceLineNo">107</span>                                } <a name="line.107"></a>
<span class="sourceLineNo">108</span>                                w[k] = wk; // w[k] - DELTA;             // return parameter w_k to original<a name="line.108"></a>
<span class="sourceLineNo">109</span>                        }<a name="line.109"></a>
<span class="sourceLineNo">110</span>                }<a name="line.110"></a>
<span class="sourceLineNo">111</span>                <a name="line.111"></a>
<span class="sourceLineNo">112</span>//                      long endtime = System.nanoTime();<a name="line.112"></a>
<span class="sourceLineNo">113</span>//                      System.out.println("time diff = " + (endtime - starttime) + " ns");<a name="line.113"></a>
<span class="sourceLineNo">114</span>//              System.out.println(NonlinearOptimizerNumeric.class.getSimpleName() + <a name="line.114"></a>
<span class="sourceLineNo">115</span>//                              ": Jacobian inverse condition number = " + MathUtil.inverseConditionNumber(J));<a name="line.115"></a>
<span class="sourceLineNo">116</span>                return J;<a name="line.116"></a>
<span class="sourceLineNo">117</span>            }<a name="line.117"></a>
<span class="sourceLineNo">118</span>                <a name="line.118"></a>
<span class="sourceLineNo">119</span>                 // THIS VERSION calculates all entries of the Jacobian (NOT USED)!<a name="line.119"></a>
<span class="sourceLineNo">120</span>            <a name="line.120"></a>
<span class="sourceLineNo">121</span>            @Deprecated<a name="line.121"></a>
<span class="sourceLineNo">122</span>            @SuppressWarnings("unused")<a name="line.122"></a>
<span class="sourceLineNo">123</span>                public double[][] value(double[] params, boolean dummy) {<a name="line.123"></a>
<span class="sourceLineNo">124</span>                //long starttime = System.nanoTime();<a name="line.124"></a>
<span class="sourceLineNo">125</span>                //System.out.println("getJacobianMatrix - NUMERICAL");<a name="line.125"></a>
<span class="sourceLineNo">126</span>                // M = number of views, N = number of model points<a name="line.126"></a>
<span class="sourceLineNo">127</span>                double[][] J = new double[2 * M * N][params.length];    // the Jacobian matrix<a name="line.127"></a>
<span class="sourceLineNo">128</span>                double[] refValues = new double[2 * M * N];     // function values obtained with undisturbed parameters <a name="line.128"></a>
<span class="sourceLineNo">129</span>                <a name="line.129"></a>
<span class="sourceLineNo">130</span>                double[] s = Arrays.copyOfRange(params, 0, camParLength);<a name="line.130"></a>
<span class="sourceLineNo">131</span>                Camera cam = new Camera(s);<a name="line.131"></a>
<span class="sourceLineNo">132</span>                <a name="line.132"></a>
<span class="sourceLineNo">133</span>                // Step 0: calculate all 2MN reference output values (for undisturbed parameters)<a name="line.133"></a>
<span class="sourceLineNo">134</span>                <a name="line.134"></a>
<span class="sourceLineNo">135</span>                for (int row = 0, m = 0; m &lt; M; m++) {  // for all views<a name="line.135"></a>
<span class="sourceLineNo">136</span>                        int start = camParLength + m * viewParLength;<a name="line.136"></a>
<span class="sourceLineNo">137</span>                                double[] w = Arrays.copyOfRange(params, start, start + viewParLength);<a name="line.137"></a>
<span class="sourceLineNo">138</span>                                ViewTransform view = new ViewTransform(w);<a name="line.138"></a>
<span class="sourceLineNo">139</span>                        for (int j = 0; j &lt; N; j++) {   // for all model points: calculate reference values<a name="line.139"></a>
<span class="sourceLineNo">140</span>                                double[] uv = cam.project(view, modelPts[j]);<a name="line.140"></a>
<span class="sourceLineNo">141</span>                                refValues[row + 0] = uv[0];<a name="line.141"></a>
<span class="sourceLineNo">142</span>                                refValues[row + 1] = uv[1];<a name="line.142"></a>
<span class="sourceLineNo">143</span>                                row = row + 2;<a name="line.143"></a>
<span class="sourceLineNo">144</span>                        }                <a name="line.144"></a>
<span class="sourceLineNo">145</span>                }<a name="line.145"></a>
<span class="sourceLineNo">146</span>                <a name="line.146"></a>
<span class="sourceLineNo">147</span>                // Step 1: calculate all entries of the Jacobian (brute force!)<a name="line.147"></a>
<span class="sourceLineNo">148</span>                <a name="line.148"></a>
<span class="sourceLineNo">149</span>                for (int k = 0; k &lt; params.length; k++) {       // for ALL parameters<a name="line.149"></a>
<span class="sourceLineNo">150</span>                        int col = k;<a name="line.150"></a>
<span class="sourceLineNo">151</span>                        // calculate step width and modify the parameter vector c:<a name="line.151"></a>
<span class="sourceLineNo">152</span>                        double pk = params[k];<a name="line.152"></a>
<span class="sourceLineNo">153</span>                        double delta = estimateDelta(pk);<a name="line.153"></a>
<span class="sourceLineNo">154</span>                        params[k] = params[k] + delta;          // modify parameter c_k<a name="line.154"></a>
<span class="sourceLineNo">155</span>                        <a name="line.155"></a>
<span class="sourceLineNo">156</span>                        double[] smod = Arrays.copyOfRange(params, 0, camParLength);<a name="line.156"></a>
<span class="sourceLineNo">157</span>                        Camera camMod = new Camera(smod);       // modified camera<a name="line.157"></a>
<span class="sourceLineNo">158</span>                        <a name="line.158"></a>
<span class="sourceLineNo">159</span>                        for (int row = 0, m = 0; m &lt; M; m++) {  // for all views<a name="line.159"></a>
<span class="sourceLineNo">160</span>                                int start = camParLength + m * viewParLength;<a name="line.160"></a>
<span class="sourceLineNo">161</span>                                double[] w = Arrays.copyOfRange(params, start, start + viewParLength);<a name="line.161"></a>
<span class="sourceLineNo">162</span>                                ViewTransform view = new ViewTransform(w);<a name="line.162"></a>
<span class="sourceLineNo">163</span>                                for (int n = 0; n &lt; N; n++) {   // for all model points: calculate disturbed value<a name="line.163"></a>
<span class="sourceLineNo">164</span>                                        Point2D Pj = modelPts[n];<a name="line.164"></a>
<span class="sourceLineNo">165</span>                                        double[] uvMod = camMod.project(view, Pj);<a name="line.165"></a>
<span class="sourceLineNo">166</span>                                        J[row + 0][col] = (uvMod[0] - refValues[row + 0]) / delta;   // du<a name="line.166"></a>
<span class="sourceLineNo">167</span>                                        J[row + 1][col] = (uvMod[1] - refValues[row + 1]) / delta;   // dv      <a name="line.167"></a>
<span class="sourceLineNo">168</span>                                        row = row + 2;<a name="line.168"></a>
<span class="sourceLineNo">169</span>                                }    <a name="line.169"></a>
<span class="sourceLineNo">170</span>                        }<a name="line.170"></a>
<span class="sourceLineNo">171</span>                        params[k] = pk;<a name="line.171"></a>
<span class="sourceLineNo">172</span>                }<a name="line.172"></a>
<span class="sourceLineNo">173</span>               <a name="line.173"></a>
<span class="sourceLineNo">174</span>                long endtime = System.nanoTime();<a name="line.174"></a>
<span class="sourceLineNo">175</span>//                      System.out.println("time diff = " + (endtime - starttime) + " ns");<a name="line.175"></a>
<span class="sourceLineNo">176</span>//                      System.out.println(this.getClass().getSimpleName() + <a name="line.176"></a>
<span class="sourceLineNo">177</span>//                              ": Jacobian inverse condition number = " + MathUtil.inverseConditionNumber(J));<a name="line.177"></a>
<span class="sourceLineNo">178</span>                return J;<a name="line.178"></a>
<span class="sourceLineNo">179</span>            }<a name="line.179"></a>
<span class="sourceLineNo">180</span><a name="line.180"></a>
<span class="sourceLineNo">181</span>        }<a name="line.181"></a>
<span class="sourceLineNo">182</span>        <a name="line.182"></a>
<span class="sourceLineNo">183</span>        /**<a name="line.183"></a>
<span class="sourceLineNo">184</span>         * Returns a positive delta value adapted to the magnitude of the parameter x<a name="line.184"></a>
<span class="sourceLineNo">185</span>         * @param x<a name="line.185"></a>
<span class="sourceLineNo">186</span>         * @return<a name="line.186"></a>
<span class="sourceLineNo">187</span>         */<a name="line.187"></a>
<span class="sourceLineNo">188</span>    private double estimateDelta(double x) {<a name="line.188"></a>
<span class="sourceLineNo">189</span>        final double eps = 1.5e-8;      // = sqrt(2.2 * 10^{-16})<a name="line.189"></a>
<span class="sourceLineNo">190</span>        double dx = eps * Math.max(Math.abs(x), 1); // dx &gt;= eps<a name="line.190"></a>
<span class="sourceLineNo">191</span>        // avoid numerical truncation problems (add and subtract again) - <a name="line.191"></a>
<span class="sourceLineNo">192</span>        // not sure if this survives the compiler !?<a name="line.192"></a>
<span class="sourceLineNo">193</span>        double tmp = x + dx;<a name="line.193"></a>
<span class="sourceLineNo">194</span>        return tmp - x;<a name="line.194"></a>
<span class="sourceLineNo">195</span>    }<a name="line.195"></a>
<span class="sourceLineNo">196</span>        <a name="line.196"></a>
<span class="sourceLineNo">197</span>}<a name="line.197"></a>
<span class="sourceLineNo">198</span><a name="line.198"></a>
<span class="sourceLineNo">199</span>/*<a name="line.199"></a>
<span class="sourceLineNo">200</span><a name="line.200"></a>
<span class="sourceLineNo">201</span>Benchmarks for calculating the Jacobian:<a name="line.201"></a>
<span class="sourceLineNo">202</span><a name="line.202"></a>
<span class="sourceLineNo">203</span>Numeric/Block<a name="line.203"></a>
<span class="sourceLineNo">204</span>time diff = 3767805 ns<a name="line.204"></a>
<span class="sourceLineNo">205</span>time diff = 2845680 ns<a name="line.205"></a>
<span class="sourceLineNo">206</span>time diff = 3000871 ns<a name="line.206"></a>
<span class="sourceLineNo">207</span>time diff = 2911613 ns<a name="line.207"></a>
<span class="sourceLineNo">208</span>time diff = 3284195 ns<a name="line.208"></a>
<span class="sourceLineNo">209</span>time diff = 1581763 ns<a name="line.209"></a>
<span class="sourceLineNo">210</span>time diff = 1582074 ns<a name="line.210"></a>
<span class="sourceLineNo">211</span><a name="line.211"></a>
<span class="sourceLineNo">212</span>Numeric/Full<a name="line.212"></a>
<span class="sourceLineNo">213</span>time diff = 9383902 ns<a name="line.213"></a>
<span class="sourceLineNo">214</span>time diff = 5680786 ns<a name="line.214"></a>
<span class="sourceLineNo">215</span>time diff = 3523978 ns<a name="line.215"></a>
<span class="sourceLineNo">216</span>time diff = 5784972 ns<a name="line.216"></a>
<span class="sourceLineNo">217</span>time diff = 3406730 ns<a name="line.217"></a>
<span class="sourceLineNo">218</span>time diff = 3463022 ns<a name="line.218"></a>
<span class="sourceLineNo">219</span>time diff = 3566275 ns<a name="line.219"></a>
<span class="sourceLineNo">220</span><a name="line.220"></a>
<span class="sourceLineNo">221</span>Analytic<a name="line.221"></a>
<span class="sourceLineNo">222</span>time diff = 7060397 ns<a name="line.222"></a>
<span class="sourceLineNo">223</span>time diff = 1106239 ns<a name="line.223"></a>
<span class="sourceLineNo">224</span>time diff = 1095976 ns<a name="line.224"></a>
<span class="sourceLineNo">225</span>time diff = 1035330 ns<a name="line.225"></a>
<span class="sourceLineNo">226</span>time diff = 1041550 ns<a name="line.226"></a>
<span class="sourceLineNo">227</span>time diff = 1043416 ns<a name="line.227"></a>
<span class="sourceLineNo">228</span>time diff = 1073895 ns<a name="line.228"></a>
<span class="sourceLineNo">229</span><a name="line.229"></a>
<span class="sourceLineNo">230</span><a name="line.230"></a>
<span class="sourceLineNo">231</span><a name="line.231"></a>
<span class="sourceLineNo">232</span><a name="line.232"></a>
<span class="sourceLineNo">233</span>*/<a name="line.233"></a>




























































</pre>
</div>
</body>
</html>
